﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using KSASupport;
using System.Windows;

namespace KSATouch
{
    /// <summary>
    /// Responsible for tracking blob motion
    /// </summary>
    public class BlobTracker
    {
        public const int TrackingThreshold = 12; // TODO: Make this settable

        Queue<List<DepthBlob>>  recentBlobs;
        List<DepthBlob>         lastFrameBlobs;
        public DepthManager     depthManager;
      
        /// <summary>
        /// Class responsible for tracking blobs, classifying them as 'down, moved, up', and turning them into Contacts
        /// </summary>
        public BlobTracker()
        {
            recentBlobs     = new Queue<List<DepthBlob>>(3);
            lastFrameBlobs  = new List<DepthBlob>();
        }

       
        /// <summary>
        /// Finds the blobs in the last frame which are closest to blobs in this frame
        /// </summary>
        /// <param name="currentBlobs"></param>
        /// <returns></returns>
        internal List<DepthBlob> MatchBlobsToClosest(List<DepthBlob> currentBlobs)
        {
            // Initialize by clearing all values in last frames blobs
            // May want to break this out into its own method
            foreach (DepthBlob blob in lastFrameBlobs)
            {
                blob.ClosestBlob = null;
                blob.ClosestDistance = -1;
                blob.IsUpdated = false;
            }
            List<DepthBlob> matchedBlobs = new List<DepthBlob>();
            // Step 1: Calculate shortest distances to currentBlobs
            foreach (DepthBlob blob in currentBlobs)
            {

                foreach (DepthBlob oldBlob in lastFrameBlobs)
                {
                    double distance = GetDistance(oldBlob.CenterPoint, blob.CenterPoint);
                    if ((blob.ClosestDistance == -1) || (distance < blob.ClosestDistance))
                    {
                        blob.ClosestDistance = distance;
                        blob.ClosestBlob = oldBlob;
                    }
                }
            }
            // Reverse the comparisons...this makes sure that the last frame's closest are measured as well as this frames
            foreach (DepthBlob blob in lastFrameBlobs)
            {
                foreach (DepthBlob newBlob in currentBlobs)
                {
                    double distance = GetDistance(newBlob.CenterPoint, blob.CenterPoint);
                    if ((blob.ClosestDistance == -1) || (distance < blob.ClosestDistance))
                    {
                        blob.ClosestDistance = distance;
                        blob.ClosestBlob = newBlob;
                    }
                }
            }
            return currentBlobs;
        }
        
        // analyze a single blob, and find out if it moved or not
        private bool IsBlobAMovedBlob(DepthBlob blob)
        {
            if(blob.ClosestDistance < TrackingThreshold)
            {
                // Check the blob that is closest to this one. Then check the blob that is closest to that one.
                // If they match, then this blob and the other are both the 'closest' and match up
                // Note: Closest blob is assigned from 'last frames blobs', so we'd update this blob with that blobs data
                if((blob.ClosestBlob != null) && (blob == blob.ClosestBlob.ClosestBlob))
                {
                    // udpate the blob
                    blob.Id = blob.ClosestBlob.Id;
                    blob.IsUpdated = true;
                    blob.LifeTime = blob.ClosestBlob.LifeTime+1; // the act of transferring means this blobs lifetime increased
                    blob.ClosestBlob.IsUpdated = true;
                    return true;
                }
            }
            return false;
        }


        /// <summary>
        /// This method will analyze contacts to find out which ones from this frame are 'Moved'. It'll update currentFramecontacts, as well as return them
        /// back.
        /// </summary>
        /// <param name="currentFrameContacts"></param>
        /// <param name="currentBlobs"></param>
        /// <returns></returns>
        internal List<Contact> ProcessContactsForMovedContacts(ref List<Contact> currentFrameContacts, List<DepthBlob> currentBlobs)
        {
            List<Contact> movedContacts = new List<Contact>();
            foreach (DepthBlob currentBlob in currentBlobs)
            {                
                int tempTrackingThreshold = TrackingThreshold;

                if(currentBlob.ClosestDistance <= tempTrackingThreshold)
                {
                    if(IsBlobAMovedBlob(currentBlob))
                    {
                        Contact movedContact = new Contact(currentBlob);
                        movedContact.CenterPoint = depthManager.Warper.Warp((float)currentBlob.CenterPoint.X, (float)currentBlob.CenterPoint.Y);
                        movedContact.CenterPoint = new Point(1920 - movedContact.CenterPoint.X, movedContact.CenterPoint.Y);
                        movedContact.ContactState = ContactStates.ContactMoved;
                        
                        movedContact.ContactBlob.MissedFrames = 0;
                        movedContact.ContactBlob.LifeTime++;                
        
                        currentFrameContacts.Add(movedContact);                        
                        movedContacts.Add(movedContact);
                    }
                }
            }
            return movedContacts;
        }

        /// <summary>
        /// Analyzes blobs which are passed in, finds which are 'new' contacts, and returns a list of them
        /// </summary>
        /// <param name="currentFrameContacts"></param>
        /// <param name="list"></param>
        /// <returns></returns>
        internal List<Contact> ProcessContactsForNewContacts(ref List<Contact> currentFrameContacts, List<DepthBlob> currentBlobs)
        {
            List<Contact> newContacts = new List<Contact>();

            foreach(DepthBlob blob in currentBlobs.Where( blob=>blob.IsUpdated == false))
            {
                // if a depth blob is big enough, it becomes a contact
                // TODO: Remove hardcoded 8
                if(blob.Points.Count > 8)
                {
                    Contact newContact = new Contact(blob);
                    newContact.CenterPoint = depthManager.Warper.Warp((float)blob.CenterPoint.X, (float)blob.CenterPoint.Y);
                    newContact.CenterPoint = new Point(1920 - newContact.CenterPoint.X, newContact.CenterPoint.Y);
                    newContact.ContactState = ContactStates.ContactDown;
                    newContact.lifetime = 0;
                    blob.IsUpdated = true;
                    blob.MissedFrames = 0;
                    currentFrameContacts.Add(newContact);
                    newContacts.Add(newContact);
                }
            }
            return newContacts;
        }
        // Updates the lastFrameBlobs for this object with the blobs which are passed in on the new frame
        internal void UpdateLastFrameBlobs(List<DepthBlob> matchedBlobs)
        {
            foreach (DepthBlob blob in matchedBlobs)
            {
                blob.IsUpdated = false;
                blob.ClosestBlob = null;
                blob.ClosestDistance = -1;
            }
            lastFrameBlobs = matchedBlobs;
        }

        /// <summary>
        /// Simpler, direct version of Processing Removed contacts without 'debounce'
        /// </summary>
        /// <param name="currentFrameContacts"></param>
        /// <param name="trackedContacts"></param>
        /// <param name="matchedBlobs"></param>
        /// <returns></returns>
        internal List<Contact> ProcessContactsForRemovedContacts(ref List<Contact> currentFrameContacts, ref List<Contact> trackedContacts, List<DepthBlob> matchedBlobs)
        {


            List<Contact> removedContacts = new List<Contact>();

            foreach (DepthBlob oldBlob in lastFrameBlobs.Where(blob => blob.IsUpdated == false))
            {
                Contact removedContact = trackedContacts.Where(con => con.Id == oldBlob.Id).SingleOrDefault();
                if (removedContact != null)
                {
                    removedContact.ContactState = ContactStates.ContactUp;
                    oldBlob.IsUpdated = true;
                    oldBlob.MissedFrames++;
                    currentFrameContacts.Add(removedContact);
                    removedContacts.Add(removedContact);
                    trackedContacts.Remove(removedContact);
                }
                else
                {
                    oldBlob.IsValid = false; // this was a 'ghost' blob that did not become a touch event, probably due to not being big enough
                }
            }
            return removedContacts;
        }

        static public double GetDistance(Point p, Point p2)
        {
            double a = (double)(p.X - p2.X);
            double b = (double)(p.Y - p2.Y);
            return Math.Sqrt((a * a) + (b * b));
        }

        #region Debug Methods
        /// <summary>
        /// Debug method to get some blob info out to the debug console. Useful if you're looking into tracking issues, but kills perf
        /// </summary>
        /// <param name="blobs">list of blobs to dump out</param>
        /// <param name="preface">something like 'hovering' or 'touch' to differentiate blobs</param>
        public void DumpBlobs(List<DepthBlob> blobs, string preface)
        {
            foreach (DepthBlob blob in blobs)
            {
                if (blob.ClosestBlob == null)
                {
                    // BLOB.ID is null!
                    System.Diagnostics.Debug.WriteLine(preface + " Blob: " + blob.Id + " Center: " + blob.CenterPoint + " ClosestID is null " + " ClosestDistance: " + blob.ClosestDistance);
                }
                else
                {
                    System.Diagnostics.Debug.WriteLine(preface + " Blob: " + blob.Id + " Center: " + blob.CenterPoint + " ClosestID" + blob.ClosestBlob.Id + " ClosestDistance: " + blob.ClosestDistance);
                }
            }
        }
        #endregion




    }
}
